
#include <agv_peripheral_controller/roller/roller_controller.h>
#include <algorithm>

#define PERRIOD_TIME 0.05 // 50ms 需要和主线程运行时间相同

namespace peripheral_roller
{
    ///////////////////////////////CRollerController 实现///////////////////////////////////////////////
    // 默认构造函数
    CRollerController::CRollerController()
    {
        m_init = false;
        m_curDistance = 0.0f;
    }

    // 析构函数
    CRollerController::~CRollerController()
    {
        m_subEvent.shutdown();
    }

    // 是否初始化完成
    bool CRollerController::initFinish()
    {
        if (!m_init)
        {
            DEBUG_ERROR_OUT("IoLifterController init failed");
        }
        return m_init;
    }

    // 初始化函数
    bool CRollerController::init()
    {
        // 订阅事件报错事件
        m_subEvent = m_priNh.subscribe("/eventstatus", 5, &CRollerController::eventCB, this);
        m_barrier.init();
        m_signals.init();
        // 电机驱动器电源
        m_signals.powerSet(true);
        m_rollerMotor.init();
        resetAutoStage();
        readMotorVelParam();
        m_init = true;
        return true;
    }

    bool CRollerController::readMotorVelParam()
    {
        m_priNh.param<float>("/agv_peripheral_controller/roller/forwardHighVel", m_motorParam.forwardHighVel, 0.0);
        m_priNh.param<float>("/agv_peripheral_controller/roller/forwardLowVel", m_motorParam.forwardLowVel, 0.0);
        m_priNh.param<float>("/agv_peripheral_controller/roller/backWardHighVel", m_motorParam.backWardHighVel, -0.0);
        m_priNh.param<float>("/agv_peripheral_controller/roller/backWardLowVel", m_motorParam.backWardLowVel, -0.0);
        m_priNh.param<float>("/agv_peripheral_controller/roller/delayFinishPicKTime", m_motorParam.delayFinishPicKTime, 5000);
        m_priNh.param<bool>("/agv_peripheral_controller/roller/openOneClickAction", m_motorParam.openOneClickAction, false);
        m_priNh.param<float>("/agv_peripheral_controller/roller/endPickDis", m_motorParam.endPickDis, 0.05);

        m_motorParam.endPickDis = abs(m_motorParam.endPickDis);
        m_motorParam.print();
        return true;
    }

    // 举升上升
    bool CRollerController::pick()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }
        m_actionName = "pick";
        // 检测是否强制一键取送货
        if (checkForcePick())
        {
            clearOc();
        }

        // 控制下降输出
        switch (m_pick)
        {
        case PickStage::AgvReady:
        {
            if (agvReadyForPick())
            {
                m_pick = PickStage::FloorReady;
                DEBUG_OUT("pick:AgvReady->FloorReady");
            }
        }
        break;
        case PickStage::FloorReady:
        {
            if (floorReadyForPick())
            {
                m_pick = PickStage::AgvFloorRunning;
                DEBUG_OUT("pick:FloorReady->AgvRunning");
            }
        }
        break;
        case PickStage::AgvFloorRunning:
        {
            if (agvAndFloorRunForPick())
            {
                m_pick = PickStage::DelayFinish;
                DEBUG_OUT("pick:AgvRunning->DelayFinish");
            }
        }
        break;
        case PickStage::DelayFinish:
        {
            if (delayFinishForPick())
            {
                m_pick = PickStage::IDLE;
                DEBUG_OUT("pick:DelayFinish->IDLE");
            }
        }
        break;
        case PickStage::ForcePick:
        {
            if (oneClickPick())
            {
                m_pick = PickStage::DelayFinish;
                DEBUG_OUT("pick:ForcePick->DelayFinish");
            }
        }
        break;
        case PickStage::ForcePause:
        case PickStage::IDLE:
        default:

            break;
        }

        // 设定光通讯真实的辊筒电机运行信号
        // setRollerMotorOcSigal();
        simCycle();
        finish = m_pick == PickStage::IDLE;
        // 完成取货，则清除光通讯
        if (finish)
        {
            clearOc();
        }
        ROS_INFO_STREAM_THROTTLE(0.2, "pick:stage=" << (int)(m_pick) << ",finish=" << finish);
        return finish;
    }

    // 举升下降
    bool CRollerController::drop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }
        m_actionName = "drop";
        // 检测是否强制一键取送货
        if (checkForceDrop())
        {
            clearOc();
        }

        switch (m_drop)
        {
        case DropStage::AgvReady:
        {
            if (agvReadyForDrop())
            {
                m_drop = DropStage::FloorReady;
                DEBUG_OUT("drop:AgvReady->FloorReady");
            }
        }
        break;
        case DropStage::FloorReady:
        {

            if (floorReadyForDrop())
            {
                m_drop = DropStage::AgvFloorRunning;
                DEBUG_OUT("drop:FloorReady->FloorRunning");
            }
        }
        break;
        case DropStage::AgvFloorRunning:
        {
            if (agvAndFloorRunForDrop())
            {
                m_drop = DropStage::IDLE;
                DEBUG_OUT("drop:AgvFloorRunning->IDLE");
            }
        }
        break;
        case DropStage::ForceDrop:
        {
            if (oneClickDrop())
            {
                m_drop = DropStage::ForceDropDelay;
                DEBUG_OUT("drop:ForceDrop->ForceDropDelay");
            }
        }
        break;
        case DropStage::ForceDropDelay:
        {
            if (delayFinishDrop())
            {
                m_drop = DropStage::IDLE;
                DEBUG_OUT("drop:ForceDropDelay->IDLE");
            }
        }
        break;

        case DropStage::ForcePause:
        case DropStage::IDLE:
        default:
            break;
        }

        // 设定光通讯真实的辊筒电机运行信号
        // setRollerMotorOcSigal();
        simCycle();
        finish = m_drop == DropStage::IDLE;
        // 完成卸货，则清除光通讯
        if (finish)
        {
            clearOc();
        }
        ROS_INFO_STREAM_THROTTLE(0.2, "drop:stage=" << (int)(m_drop) << ",finish=" << finish);
        return finish;
    }

    // 所有设备停止-并复位输出所有信号
    bool CRollerController::stop()
    {
        bool finish = false;
        if (!initFinish())
        {
            return finish;
        }

        bool ret = true;
        ret &= m_rollerMotor.stop();
        ret &= m_barrier.stop();
        finish = ret;

        if (finish)
        {
            DEBUG_WARN_OUT("stop CRollerController ok");
        }
        // 设定光通讯真实的辊筒电机运行信号
        // setRollerMotorOcSigal();
        simCycle();
        return finish;
    }

    bool CRollerController::checkForcePick()
    {
        bool forcePick = m_signals.getHmi(HmiSig::pick);
        bool forcePause = m_signals.getHmi(HmiSig::pause);
        bool forceFinish = m_signals.getHmi(HmiSig::finish);
        // 强制取货
        if (forcePick)
        {
            DEBUG_WARN_OUT("forcePick occur");
            bool allow = m_pick != PickStage::ForcePick; // && m_pick != PickStage::DelayFinish && m_pick != PickStage::IDLE;
            // 一键取货强行介入
            if (allow)
            {
                DEBUG_WARN_OUT("pick: force enter forcePick:last=" << (int)m_pick);
                stop();
                m_pick = PickStage::ForcePick;
                return true;
            }
        }

        // 强制暂停
        if (forcePause)
        {
            bool allow = m_pick == PickStage::ForcePick; // && m_pick != PickStage::DelayFinish && m_pick != PickStage::IDLE;
            DEBUG_WARN_OUT("forcePause occur:allow=" << allow);
            // 一键z暂停强行介入
            if (allow)
            {
                DEBUG_WARN_OUT("pick: force enter forcePause last is forcePick");
                stop();
                m_pick = PickStage::ForcePause;
                return true;
            }
        }

        // 强制完成
        if (forceFinish)
        {
            bool allow = m_pick == PickStage::ForcePick && m_pick == PickStage::DelayFinish;
            bool loadOk = cargoBits() == CargoBitState::Stop;
            bool upOk = m_barrier.upTarget();
            DEBUG_WARN_OUT("forceFinish occur:allow=" << allow << ",loadOk=" << loadOk << ",upOk=" << upOk);
            if (allow && loadOk && upOk)
            {
                DEBUG_WARN_OUT("pick: force enter forceFinish last is=" << (int)m_pick);
                stop();
                m_pick = PickStage::IDLE;
                return true;
            }
        }

        return false;
    }

    bool CRollerController::checkForceDrop()
    {
        bool forceDrop = m_signals.getHmi(HmiSig::drop);
        bool forcePause = m_signals.getHmi(HmiSig::pause);
        bool forceFinish = m_signals.getHmi(HmiSig::finish);
        // 强制卸货
        if (forceDrop)
        {
            DEBUG_WARN_OUT("forceDrop occur");
            bool allow = m_drop != DropStage::ForceDrop && m_drop != DropStage::ForceDropDelay; // && m_drop != PickStage::IDLE;
            // 一键取货强行介入
            if (allow)
            {
                DEBUG_WARN_OUT("drop: force enter forceDrop:last=" << (int)m_drop);
                stop();
                m_drop = DropStage::ForceDrop;
                return true;
            }
        }

        // 强制暂停
        if (forcePause)
        {
            bool allow = m_drop == DropStage::ForceDrop || m_drop == DropStage::ForceDropDelay; // && m_drop != PickStage::DelayFinish && m_drop != PickStage::IDLE;
            DEBUG_WARN_OUT("drop:forcePause occur:allow=" << allow);
            // 一键z暂停强行介入
            if (allow)
            {
                DEBUG_WARN_OUT("drop: force enter forcePause last is=" << (int)m_drop);
                stop();
                m_drop = DropStage::ForcePause;
                return true;
            }
        }

        // 强制完成
        if (forceFinish)
        {
            bool allow = m_drop == DropStage::ForceDrop || m_drop == DropStage::ForceDropDelay;
            bool unloadOk = m_signals.cargoInputBits(true) == 0;
            bool downOk = m_barrier.downTarget();
            DEBUG_WARN_OUT("drop:forceFinish occur:allow=" << allow << ",unloadOk=" << unloadOk << ",downOk=" << downOk);
            if (allow && unloadOk && downOk)
            {
                DEBUG_WARN_OUT("drop : force enter forceFinish last is=" << (int)m_drop);
                stop();
                m_drop = DropStage::IDLE;
                return true;
            }
        }

        return false;
    }

    bool CRollerController::clearOc()
    {
        DEBUG_OUT("clearOc once");
        return m_signals.ocOutputReset();
    }

    void CRollerController::resetAutoStage()
    {
        m_runFault = AutoRunErr::No;
        m_pick = PickStage::Unkown;
        m_drop = DropStage::Unkown;
        clearOc();
        m_curDistance = 0.0f;
        DEBUG_OUT("RollerController reset once");
    }

    bool CRollerController::readyStart(CParameters params)
    {
        m_params = params;
        m_runFault = AutoRunErr::No;
        m_pick = PickStage::AgvReady;
        m_drop = DropStage::AgvReady;
        clearOc();
        readMotorVelParam();
        DEBUG_OUT("RollerController reset all signal start once");
        return stop();
    }

    // 货物装填
    bool CRollerController::loadStatus()
    {
        bool load = false;
        if (!initFinish())
        {
            return load;
        }
        // 货物是否载入
        load = m_signals.loadStatus();
        return load;
    }

    void CRollerController::eventCB(const agv_msgs::msg::AGVEventStatus &event)
    {
        m_eventStatus = event;
        static int lastId = 0xFFFF;
        if (event.ecode != lastId)
        {
            DEBUG_WARN_OUT("receive event:" << event.ecode << ",details=" << event.description
                                            << ",ctrol_mode=" << (int)event.action);
        }
        lastId = event.ecode;
    }

    bool CRollerController::emgEventOccur()
    {
        auto curEventStatus = m_eventStatus;
        // 只有拍下急停才会触发停止---EMG_EVENT
        bool emgAction = curEventStatus.action == agv_msgs::msg::AGVEvent::ESTOP;
        bool emgEvent = curEventStatus.ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT;
        bool ret = emgAction && emgEvent;

        // 如果当前最高优先级的事件不是急停，则从事件列表内查询
        if (emgAction && !emgEvent)
        {
            auto events = curEventStatus.events;
            auto it = std::find_if(events.begin(), events.end(), [](const agv_msgs::msg::AGVEvent &event)
                                   { return event.ecode == agv_msgs::msg::AGVEvent::EMG_STOP_EVENT; });

            ret = it != events.end();
        }

        if (ret)
        {
            ROS_WARN_STREAM_THROTTLE(0.3, "RollerController eventOccur: emg occur");
        }

        return ret;
    }

    void CRollerController::simCycle()
    {
        m_signals.simOcOutCycle();
    }

    // 是否设备报错
    bool CRollerController::fault()
    {
        bool e1 = m_barrier.fault();
        bool e2 = m_rollerMotor.fault();
        bool e3 = autoRunFault();
        if (e1 || e2 || e3)
        {
            ROS_ERROR_STREAM_THROTTLE(0.5, "RollerController has error: barrierError=" << e1 << ",motorError=" << e2
                                                                                       << ",autoRunFault=" << (int)m_runFault);
        }

        return e1 || e2 || e3;
    }

    // 是否设备报错
    std::string CRollerController::faultMsg()
    {
        auto msg1 = m_barrier.faultMsg();
        auto msg2 = m_rollerMotor.faultMsg();
        auto msg3 = autoRunFaultMsg();
        return msg1 + msg2 + msg3;
    }

    bool CRollerController::clearFault()
    {
        static bool last = false;
        bool cur = m_signals.resetBtnPressed();
        bool ret1 = false;
        bool ret2 = false;
        if (cur && last != cur)
        {
            bool e1 = m_barrier.fault();
            bool e2 = m_rollerMotor.fault();
            bool e3 = autoRunFault();
            if (e1)
            {
                ret1 = m_barrier.clearFault();
                DEBUG_OUT("clearFault barrier ret1=" << ret1);
            }

            if (e2)
            {
                ret2 = m_rollerMotor.clearFault();
                DEBUG_OUT("clearFault rollerMotor ret2=" << ret2);
            }

            if (e3)
            {
                clearAutoRunFault();
            }
        }
        last = cur;
        return ret1 || ret2;
    }

    int CRollerController::curStep()
    {
        int step = m_actionName == "pick" ? (int)m_pick : (int)m_drop;
        return step;
    }

    std::vector<agv_msgs::msg::PeriphMotorInfo> CRollerController::periphMotor()
    {
        return m_rollerMotor.motorInfoCollect();
    }

    // 输出是否处于正常的运行状态(不作为滚筒的实际运行状态)
    bool CRollerController::setRollerMotorOcSigal(bool run)
    {
        bool ret = m_signals.ocOutput(AgvSig::AgvRollerRunning, run);
        return ret;
    }

    // 业务逻辑报警-地面辊道异常停止等错误
    bool CRollerController::autoRunFault()
    {
        return m_runFault != AutoRunErr::No;
    }

    // 清除自动运行错误
    void CRollerController::clearAutoRunFault()
    {
        m_runFault = AutoRunErr::No;
        DEBUG_OUT("clearAutoRunFault once");
    }

    // 业务逻辑报警-地面辊道异常停止等错误
    std::string CRollerController::autoRunFaultMsg()
    {
        std::string msg;
        switch (m_runFault)
        {
        case AutoRunErr::No:
            msg = "";
            break;

        case AutoRunErr::FloorStop:
            msg = "floor roller motor stop ";
            break;
        case AutoRunErr::FloorAbnormal:
            msg = "floor roller normal signal miss";
            break;
        case AutoRunErr::DtMiss:
            msg = "DT signal miss";
            break;
        default:
            break;
        }
        return msg;
    }

    //
    // 货物开关的状态
    //
    CargoBitState CRollerController::cargoBits()
    {
        // 如果无检测开关，则此逻辑无法执行
        uint64_t bits = m_signals.cargoInputBits(true);
        int count = m_signals.cargoInputSize();
        if (count == 0)
        {
            DEBUG_ERROR_OUT("error: cargoInputSize is 0");
            return CargoBitState::High;
        }

        // 如果最后一个开关触发-stop-且倒数第二个(若存在)则必须也触发
        bool stop = bits >= BIT(count - 1);

        //  倒数第二个开关降速-low; 其他的-高速-high
        bool low = true;         // 低速
        bool lowBitOccur = true; // 倒数第二个开关(若存在)
        if (count >= 2)
        {
            low = bits >= BIT(count - 2);
            lowBitOccur = TestBit(bits, count - 2);
        }

        // 必须至少最后两个开关检测到，才认为真正检测到，防止误触发停止
        if (stop && lowBitOccur)
        {
            DEBUG_OUT("cargoBits: stop bits" << bits);
            return CargoBitState::Stop;
        }

        if (low)
        {
            DEBUG_OUT("cargoBits: low bits" << bits);
            return CargoBitState::Low;
        }

        return CargoBitState::High;
    }

    ////////////////////////pick 接料过程/////////////////////////
    //
    // #1 agv就绪
    //
    bool CRollerController::agvReadyForPick()
    {
        bool ret = false;
        //  检测光通讯信号是否是正常
        if (!m_signals.ocDataT())
        {
            DEBUG_ERROR_OUT("error:agvReadyForPick oc Input is not Normal;DT=" << m_signals.ocDataT()
                                                                               << ",ocInputNormal=" << m_signals.ocInputNormal());
            return ret;
        }

        // 检测agv上是否有货物
        bool load = m_signals.cargoInputBits(true) > 0;
        if (load)
        {
            DEBUG_ERROR_OUT("error:agvReadyForPick cargo is not empty;can not pick");
            return ret;
        }

        // 阻挡下降到位
        if (m_barrier.down())
        {
            // #1 光通讯输出接料正常信号-out2
            ret = m_signals.ocOutput(AgvSig::PickFromFloorReady, true);
        }

        DEBUG_OUT_THROTTLE(0.1, "agvReadyForPick: barrrer is downing...ret=" << ret);

        return ret;
    }

    //
    // #2 地面就绪
    //
    bool CRollerController::floorReadyForPick()
    {
        bool ret = m_signals.ocInput(FloorSig::DropToAgvReady);
        DEBUG_OUT_THROTTLE(0.1, "floorReadyForPick...wait floor drop signal;ret=" << ret);
        return ret;
    }

    //
    // #3 agv+地面辊道运行
    //
    bool CRollerController::agvAndFloorRunForPick()
    {

        //  检测光通讯信号是否是正常
        static int dtMissCount = 0; // 50*20 =1000ms
        if (!m_signals.ocDataT() && dtMissCount++ > 20)
        {
            m_rollerMotor.stop();
            m_runFault = AutoRunErr::DtMiss;
            DEBUG_ERROR_OUT("error:agvAndFloorRunForPick DtMiss;dtMissCount=" << dtMissCount);
            return false;
        }
        else
        {
            dtMissCount = 0;
        }

        // 一直检测地面辊道是否运行，如果不运行，停止并报错
        bool normal = m_signals.ocInput(FloorSig::DropToAgvReady);
        if (!normal)
        {
            m_rollerMotor.stop();
            m_runFault = AutoRunErr::FloorAbnormal;
            DEBUG_ERROR_OUT("error:waitFinishForPick FloorAbnormal");
            return false;
        }

        // 设定正常运行信号-(与agv实际辊筒信号不符合)
        setRollerMotorOcSigal(true);

        // 设定速度
        float vel = 0.0f;
        bool readyStop = stepVelPick(vel) == CargoBitState::Stop;
        // #4 光通讯地面辊道运行中信号-in3
        bool floorRun = m_signals.ocInput(FloorSig::FloorRollerRunning);
        if (!floorRun)
        {
            DEBUG_WARN_OUT("wait floor motor run signal....");
            m_rollerMotor.stop();
            return false;
        }

        // agv辊筒电机反转-取料
        m_rollerMotor.rotate(vel);

        // 准备延迟结束
        if (readyStop)
        {
            // 该阶段将正常退出,复位定时器，为下个阶段延迟准备
            m_timer.Stop();
            m_signals.ocOutput(AgvSig::PickFromFloorFinish, true);
            m_curDistance = 0.0f;
            DEBUG_OUT("waitFinishForPick stop enter curDistance ++,set PickFromFloorFinish true");
            return true;
        }

        return false;
    }

    //
    // #4 继续补偿行走的距离取货完成
    // 停止辊筒+阻挡上升到位
    //
    bool CRollerController::delayFinishForPick()
    {

        bool ret = false;
        // 此处距离必须为正值
        m_curDistance += PERRIOD_TIME * abs(m_motorParam.backWardLowVel);
        if (m_curDistance >= m_motorParam.endPickDis)
        {
            bool motoStop = m_rollerMotor.stop();
            bool up = m_barrier.up();
            if (motoStop && up)
            {
                stop(); // 停止并清除信号
                ret = true;
                DEBUG_OUT("pick:AgvSig::delayFinishForPick ok; barrier up finish m_curDistance=" << m_curDistance);
            }

            DEBUG_OUT_THROTTLE(0.1, "pick:delayFinishForPick;ret=" << ret << ",motoStop=" << motoStop << ",up=" << up);
        }

        DEBUG_OUT("delayFinishForPick stop bits(?=2)=" << (int)cargoBits()
                                                       << ",m_curDistance=" << m_curDistance);
        return ret;
    }

    // 一键取货
    //  从自动模式切换到此模式时，需要先stop一下在执行(外部逻辑)
    bool CRollerController::oneClickPick()
    {
        bool ret = false;

        // 不开启一键取送货功能
        if (!m_motorParam.openOneClickAction)
        {
            DEBUG_OUT_THROTTLE(1, "oneClickPick openOneClickAction close");
            return ret;
        }

        // #1 阻挡下降未到位,停止normal信号，停止辊道
        if (!m_barrier.down())
        {
            m_rollerMotor.stop();
            DEBUG_OUT_THROTTLE(0.1, "oneClickPick: barrier down running..");
            return ret;
        }

        // 辊筒运行，直至检测到货物
        float vel = 0.0f;
        CargoBitState bits = stepVelPick(vel);
        ret = bits == CargoBitState::Stop;

        if (ret)
        {
            m_rollerMotor.stop();
            m_timer.Stop();
            DEBUG_OUT("oneClickPick finish wait barrier up...");
        }
        else
        {
            m_rollerMotor.rotate(vel);
            DEBUG_OUT_THROTTLE(0.1, "oneClickPick: running.. vel=" << vel);
        }
        return ret;
    }

    // 取货时，电机速度阶梯
    CargoBitState CRollerController::stepVelPick(float &fRunVel)
    {

        // #4 agv先尝试转1s，等待辊道也同步转，否则停止
        CargoBitState bits = cargoBits();
        // 最后一个货物检测开关触发，停止辊道，完成该阶段
        float vel = m_motorParam.backWardLowVel;
        switch (bits)
        {
        case CargoBitState::High:
            vel = m_motorParam.backWardHighVel;
            break;
        case CargoBitState::Low:
            vel = m_motorParam.backWardLowVel;
            break;
        case CargoBitState::Stop:
            // vel = 0.0f;
            break;
        default:
            break;
        }

        fRunVel = vel;

        // bool rotate = m_rollerMotor.rotate(vel);
        // if (!rotate)
        // {
        //     DEBUG_ERROR_OUT("stepVelForPick:motor rotate failed; vel=" << vel);
        // }

        DEBUG_OUT_THROTTLE(0.1, "stepVelForPick: vel=" << vel << ",bits=" << (int)bits);

        return bits;
    }

    ////////////////////////drop 送料过程/////////////////////////////

    //
    // #1 agv就绪
    //
    bool CRollerController::agvReadyForDrop()
    {
        bool ret = false;

        //  检测光通讯信号是否是正常
        if (!m_signals.ocDataT())
        {
            DEBUG_ERROR_OUT("error:agvReadyForDrop oc Input is not Normal;DT=" << m_signals.ocDataT()
                                                                               << ",ocInputNormal=" << m_signals.ocInputNormal());
            return ret;
        }

        // 检测agv上是否有货物
        bool load = m_signals.cargoInputBits(true) > 0;
        if (!load)
        {
            DEBUG_ERROR_OUT("error:agvReadyForDrop cargo is empty;can not drop");
            return ret;
        }

        // 阻挡下降到位
        if (m_barrier.down())
        {
            // #1 光通讯输出agv卸料正常信号-out1
            ret = m_signals.ocOutput(AgvSig::DropToFloorReady, true);
        }

        DEBUG_OUT_THROTTLE(0.1, "agvReadyForDrop: barrrer is downing...ret=" << ret);

        return ret;
    }

    //
    // #2 地面就绪
    //
    bool CRollerController::floorReadyForDrop()
    {
        // #2 光通讯地面辊道接料正常信号-in2
        bool floorReady = m_signals.ocInput(FloorSig::PickFromAgvReady);
        // #3 光通讯地面辊道运行中信号-in3
        bool floorRun = m_signals.ocInput(FloorSig::FloorRollerRunning);
        DEBUG_OUT_THROTTLE(0.1, "floorReadyForDrop wait floor signal..." << " floorReady=" << floorReady
                                                                         << ",floorRun=" << floorRun);
        bool ret = floorReady && floorRun;
        if (ret)
        {
            m_timer.Stop();
        }
        return ret;
    }

    //
    // #3 agv辊道运行->直至卸货完成
    //
    bool CRollerController::agvAndFloorRunForDrop()
    {

        //  检测光通讯信号是否是正常
        static int dtMissCount = 0; // 50*20 =1000ms
        if (!m_signals.ocDataT() && dtMissCount++ > 20)
        {
            m_rollerMotor.stop();
            m_runFault = AutoRunErr::DtMiss;
            DEBUG_ERROR_OUT("error:agvAndFloorRunForDrop DtMiss:dtMissCount=" << dtMissCount);
            return false;
        }
        else
        {
            dtMissCount = 0;
        }

        // 一直检测地面辊道是否运行，如果不运行，停止并报错
        bool normal = m_signals.ocInput(FloorSig::PickFromAgvReady);
        bool floorRun = m_signals.ocInput(FloorSig::FloorRollerRunning);
        if (!normal)
        {
            m_rollerMotor.stop();
            m_runFault = AutoRunErr::FloorAbnormal;
            m_timer.Stop();
            DEBUG_ERROR_OUT("drop error:agvAndFloorRunForDrop normal=" << normal << ",floorRun=" << floorRun
                                                                       << ",runFault=" << (int)m_runFault);
            return false;
        }

        // 设置辊筒运行中信号;
        setRollerMotorOcSigal(true);

        // 货物空+辊道接料完成
        uint64_t bits = m_signals.cargoInputBits(true);
        bool cargoEmpty = bits == 0;
        // // #5 光通讯地面辊道接货完成-in4
        bool floorFinish = m_signals.ocInput(FloorSig::PickFromAgvFinish);
        // 先判断完成，在判断是否未完成，辊筒异常停止
        if (cargoEmpty && floorFinish)
        {
            m_rollerMotor.stop();
            stop();
            DEBUG_OUT("ok!!agvAndFloorRunForDrop: finish...");
            return true;
        }

        // 如果货物还在，对方辊筒异常停止，则报错
        if (!cargoEmpty && !floorRun)
        {
            DEBUG_WARN_OUT("drop: wait floor motor run signal....");
            m_rollerMotor.stop();
            return false;
        }

        // agv辊筒电机正转-准备送料
        m_rollerMotor.rotate(m_motorParam.forwardHighVel);

        DEBUG_OUT_THROTTLE(0.1, "agvAndFloorRunForDrop:cargoEmpty= " << cargoEmpty
                                                                     << ",floorFinish=" << floorFinish
                                                                     << ",bits=" << bits);

        return false;
    }

    // 一键卸货逻辑
    //  卸货时，电机速度阶梯
    CargoBitState CRollerController::stepVelDrop()
    {
        // 所有货物检测开关都未触发，则货物送完
        // 停止辊道，完成该阶段
        uint64_t bits = m_signals.cargoInputBits(true);
        bool cargoEmpty = bits == 0;

        // 地面辊道接料完成，则需要延迟停止agv辊道
        float vel = m_motorParam.forwardHighVel;
        bool rotate = m_rollerMotor.rotate(vel);
        if (!rotate)
        {
            DEBUG_ERROR_OUT("stepVelForDrop:motor rotate failed; vel=" << vel);
        }
        DEBUG_OUT_THROTTLE(0.1, "stepVelForDrop: vel=" << vel << ",cargoEmpty=" << cargoEmpty);
        CargoBitState ret = cargoEmpty ? CargoBitState::Stop : CargoBitState::High;
        return ret;
    }

    // 一键卸货
    //  从自动模式切换到此模式时，需要先stop一下在执行(外部逻辑)
    bool CRollerController::oneClickDrop()
    {
        bool ret = false;

        if (!m_motorParam.openOneClickAction)
        {
            DEBUG_OUT_THROTTLE(1, "oneClickDrop openOneClickAction close");
            return ret;
        }

        // 不检测agv上是否有货物-直接满足条件最低转5s
        // 阻挡下降到位
        if (!m_barrier.down())
        {
            m_rollerMotor.stop();
            DEBUG_OUT_THROTTLE(0.1, "oneClickDrop: barrier down running..");
            return ret;
        }

        // agv辊筒电机正转-准备送料
        if (CargoBitState::Stop == stepVelDrop())
        {
            m_timerDelay.Stop();
            ret = true;
            DEBUG_OUT("oneClickDrop:stepVelForDrop ok,wait 5...");
        }
        return ret;
    }

    // 一键卸货时，延时5s结束
    bool CRollerController::delayFinishDrop()
    {
        m_timerDelay.Start();
        bool timerOccur = m_timerDelay.TriggerAlways(5000);
        if (timerOccur)
        {
            m_rollerMotor.stop();
            DEBUG_OUT("delaylStopForDrop:5s timerOccur  motor stop");
            return true;
        }

        // 地面辊道接料完成，则需要延迟停止agv辊道
        float vel = m_motorParam.forwardHighVel;
        bool rotate = m_rollerMotor.rotate(vel);
        if (!rotate)
        {
            DEBUG_ERROR_OUT("delaylStopForDrop:motor rotate failed; vel=" << vel);
        }
        DEBUG_OUT_THROTTLE(0.1, "delaylStopForDrop: vel=" << vel);

        return false;
    }

    ////////////////////////////手动状态下循环执行///////////////////////////////////

    void CRollerController::barrierManul()
    {
        std::string cmd = "";
        bool finish = false;
        SwitchState btn = m_barrier.switchBtn();
        switch (btn)
        {
        case SwitchState::UP:
            finish = m_barrier.up();
            cmd = "up:";
            break;
        case SwitchState::DOWN:
            finish = m_barrier.down();
            cmd = "down:";
            break;
        case SwitchState::STOP:
        default:
            finish = m_barrier.stop();
            cmd = "stop:";
            break;
        }
        ROS_INFO_STREAM_THROTTLE(0.5, "barrierManul: cmd:" << cmd << ",finish=" << finish);
    }

    // 辊筒电机手动控制
    void CRollerController::motorManul()
    {
        // 读取电机参数速度
        readMotorVelParam();

        std::string cmd = "";
        bool finish = false;
        SwitchState btn = m_signals.switchBtn();
        switch (btn)
        {
        case SwitchState::FORWARD:
            finish = m_rollerMotor.rotate(m_motorParam.forwardHighVel);
            cmd = "forward:";
            break;
        case SwitchState::REVERSE:
            finish = m_rollerMotor.rotate(m_motorParam.backWardHighVel);
            cmd = "reverse:";
            break;
        case SwitchState::STOP:
        default:
            finish = m_rollerMotor.stop();
            cmd = "stop:";
            break;
        }
        ROS_INFO_STREAM_THROTTLE(0.5, "motorManul: cmd:" << cmd << ",finish=" << finish);
    }

    void CRollerController::manulControl()
    {
        // 阻挡手动操作
        barrierManul();

        // 辊筒电机手动操作
        motorManul();

        simCycle();
    }
}; // end namespace
